#include "color.h"

void ImgBGR2Lab(void* pu8Src,int rows,int cols,void *pf32Dst)
{
	uchar* pTmpSrc = (uchar*)pu8Src;
	float* pTmpDst = (float*)pf32Dst;
	for(int i = 0 ; i < rows ; i++)
	{
		for(int j = 0 ; j< cols ; j++, pTmpDst+=3, pTmpSrc+=3)
		{
			RGB2Lab_pixel(pTmpSrc[2],pTmpSrc[1],pTmpSrc[0],pTmpDst);
		}
	}
	return;
}


void RGB2Lab_pixel(uchar R, uchar G, uchar B,float* pDst)
{
	if(R==0 && G==0 && B==0)
	{
		pDst[0] = 0;
		pDst[1] = 0;
		pDst[2] = 0;
		return;
	}
	float r = R*1.0f / 255.0f,
		  g = G*1.0f / 255.0f,
		  b = B*1.0f / 255.0f,
		  x, y, z;

	r = (r > 0.04045f) ? pow((r + 0.055f) / 1.055f, 2.4f) : r / 12.92f;
	g = (g > 0.04045f) ? pow((g + 0.055f) / 1.055f, 2.4f) : g / 12.92f;
	b = (b > 0.04045f) ? pow((b + 0.055f) / 1.055f, 2.4f) : b / 12.92f;

	x = (r * 0.4124f + g * 0.3576f + b * 0.1805f) / 0.95047f;
	y = (r * 0.2126f + g * 0.7152f + b * 0.0722f) / 1.00000f;
	z = (r * 0.0193f + g * 0.1192f + b * 0.9505f) / 1.08883f;

	x = (x > 0.008856f) ? pow(x, 1.0f/3.0f) : (7.787f * x) + 16.0f/116;
	y = (y > 0.008856f) ? pow(y, 1.0f/3.0f) : (7.787f * y) + 16.0f/116;
	z = (z > 0.008856f) ? pow(z, 1.0f/3.0f) : (7.787f * z) + 16.0f/116;

	pDst[0] = (116.0f * y) - 16.0f;
	pDst[1] = 500.0f * (x - y);
	pDst[2] = 200.0f * (y - z);
	return;
}

void CalcDaAndDb(void* pf32Src, int height, int width, float* Da, float* Db)
{
	float* pTmp = (float*) pf32Src;
	for(int i = 0 ; i < height ; i ++)
	{
		for(int j = 0 ; j < width ; j++, pTmp+=3)
		{
			*Da+=pTmp[1];
			*Db+=pTmp[2];
		}
	}
	*Da/=(height*width);
	*Db/=(height*width);
	return;
}

void CalcMaAndMb(void* pf32Src, int height, int width,float Da, float Db, float* Ma, float* Mb)
{
	float* pTmp = (float*) pf32Src;
	for(int i = 0 ; i < height ; i ++)
	{
		for(int j = 0 ; j < width ; j++, pTmp+=3)
		{
			*Ma+=(pTmp[1]-Da)*(pTmp[1]-Da);
			*Mb+=(pTmp[2]-Db)*(pTmp[2]-Db);
		}
	}
	*Ma/=(height*width);
	*Mb/=(height*width);
	return;
}

void ColorCorrect(void* pf32Src, int height, int width,float Da, float Db)
{
	float* pTmp = (float*) pf32Src;
	for(int i = 0 ; i < height ; i ++)
	{
		for(int j = 0 ; j < width ; j++, pTmp+=3)
		{
			pTmp[1]-=Da;
			pTmp[2]-=Db;
		}
	}
	return;
}

void ImgLab2BGR(void* pf32Src,int height,int width,void *pu8Dst)
{
	float* pTmpSrc =(float*) pf32Src;
	uchar* pTmpDst =(uchar*) pu8Dst;

	for(int i = 0 ; i < height ; i ++)
	{
		for(int j = 0 ; j < width ; j++, pTmpDst+=3,pTmpSrc+=3)
		{
			Lab2BGR_pixel(pTmpSrc,pTmpDst);
		}
	}
	return;
}

void Lab2BGR_pixel(void* pf32Src, void *pu8Dst)
{
	float *Lab = (float*)pf32Src;
	float y = (Lab[0] + 16) / 116,
		x = Lab[1] / 500 + y,
		z = y - Lab[2] / 200,
		r, g, b;

	x = 0.95047f * ((x * x * x > 0.008856f) ? x * x * x : (x - 16.0f/116) / 7.787f);
	y = 1.00000f * ((y * y * y > 0.008856f) ? y * y * y : (y - 16.0f/116) / 7.787f);
	z = 1.08883f * ((z * z * z > 0.008856f) ? z * z * z : (z - 16.0f/116) / 7.787f);

	r = x *  3.2406f + y * -1.5372f + z * -0.4986f;
	g = x * -0.9689f + y *  1.8758f + z *  0.0415f;
	b = x *  0.0557f + y * -0.2040f + z *  1.0570f;

	r = (r > 0.0031308f) ? (1.055f * pow(r, 1.0f/2.4f) - 0.055f) : 12.92f * r;
	g = (g > 0.0031308f) ? (1.055f * pow(g, 1.0f/2.4f) - 0.055f) : 12.92f * g;
	b = (b > 0.0031308f) ? (1.055f * pow(b, 1.0f/2.4f) - 0.055f) : 12.92f * b;

	uchar* RGB=(uchar*)pu8Dst;
	RGB[2] = MAX(0, MIN(1, r)) * 255;
	RGB[1] = MAX(0, MIN(1, g)) * 255;
	RGB[0] = MAX(0, MIN(1, b)) * 255;
	return;
}